////////////////////////////////////////////////////////////////////////////////
// Description: 2012 E-Week Competition: Roboditch
// Author:      Michael Colina
// Date:        November 06, 2011
////////////////////////////////////////////////////////////////////////////////
#include <Servo.h> 

# define DEBUG 1
# define SIM 0

unsigned int const BUFF_SIZE_BYTES = 256;
char mSerialBuffer[BUFF_SIZE_BYTES];

// Pin Map
// Pin 0 Rx
// Pin 1 Tx
// Pin 2 Unused
// Pin 3 Unused (PWM)
// Pin 4 Unused
// Pin 7 Unused
// Pin 10 Unused (PWM) (currently broken)
unsigned int const IN_DOOR_PIN      = 5;  // PWM
unsigned int const OUT_DOOR_PIN     = 6;  // PWM
unsigned int const DOORS_TOGGLE_PIN = 8;
unsigned int const FAN_TOGGLE_PIN   = 9;  // PWM
unsigned int const BRIDGE_EN_PIN    = 11; // PWM
unsigned int const BRIDGE_IN1_PIN   = 12;
unsigned int const BRIDGE_IN2_PIN   = 13;

// Servo Instances
Servo mInDoorServo;
Servo mOutDoorServo;

// Values for determining position of switches on RC Controller based on
// outputs from the RC Receiver
unsigned int const PULSE_ERROR    = 50;
unsigned int const SWITCH_OFF_AVG = 1180;
unsigned int const SWITCH_ON_AVG  = 1680;
unsigned int const SUCK_AVG       = 1080;
unsigned int const BLOW_AVG       = 1880;

unsigned int const CHANGE_DIR_DELAY_MS = 1000;

unsigned int const SWITCH_OFF_MIN  = SWITCH_OFF_AVG - PULSE_ERROR;
unsigned int const SWITCH_OFF_MAX  = SWITCH_OFF_AVG + PULSE_ERROR;

unsigned int const SWITCH_ON_MIN = SWITCH_ON_AVG - PULSE_ERROR;
unsigned int const SWITCH_ON_MAX = SWITCH_ON_AVG + PULSE_ERROR;

unsigned int const SUCK_MIN      = SUCK_AVG - PULSE_ERROR;
unsigned int const SUCK_MAX      = SUCK_AVG + PULSE_ERROR;

unsigned int const BLOW_MIN      = BLOW_AVG - PULSE_ERROR;
unsigned int const BLOW_MAX      = BLOW_AVG + PULSE_ERROR;

unsigned int mDoorsValue = 0;q
unsigned int mFanValue   = 0;

// Constants to control door servos
// Orig Values
//unsigned int const IN_DOOR_CLOSED_ANGLE  = 160;
//unsigned int const IN_DOOR_OPEN_ANGLE    = 100;
//unsigned int const OUT_DOOR_CLOSED_ANGLE = 158;
//unsigned int const OUT_DOOR_OPEN_ANGLE   = 40;

unsigned int const IN_DOOR_CLOSED_ANGLE  = 147;
unsigned int const IN_DOOR_OPEN_ANGLE    = 100;
unsigned int const OUT_DOOR_CLOSED_ANGLE = 146;
unsigned int const OUT_DOOR_OPEN_ANGLE   = 40;

unsigned int const DOOR_DELAY_MS         = 750;

enum SYSTEM_STATE_ENUM
{
  IDLE,
  ACQUIRING,
  RELEASING
};
SYSTEM_STATE_ENUM mSystemState = IDLE;


enum DOORS_STATE_ENUM
{
  ACQUIRE,
  RELEASE
};
DOORS_STATE_ENUM mPrevDoorsState = ACQUIRE;
DOORS_STATE_ENUM mCurrDoorsState = ACQUIRE;


////////////////////////////////////////////////////////////////////////////////
// Function:    void OpenInDoor()
// Description: This function is used to open the input door.
////////////////////////////////////////////////////////////////////////////////
void OpenInDoor()
{
  mInDoorServo.write( IN_DOOR_OPEN_ANGLE );
  delay(DOOR_DELAY_MS);
}

////////////////////////////////////////////////////////////////////////////////
// Function:    void CloseInDoor()
// Description: This function is used to close the input door.
////////////////////////////////////////////////////////////////////////////////
void CloseInDoor()
{
  mInDoorServo.write( IN_DOOR_CLOSED_ANGLE );
  delay( DOOR_DELAY_MS );
}

////////////////////////////////////////////////////////////////////////////////
// Function:    void OpenOutDoor()
// Description: This function is used to open the output door.
////////////////////////////////////////////////////////////////////////////////
void OpenOutDoor()
{
  mOutDoorServo.write( OUT_DOOR_OPEN_ANGLE );
  delay( DOOR_DELAY_MS );
}

////////////////////////////////////////////////////////////////////////////////
// Function:    void DoorsAcquire()
// Description: This function is used to change the position of the doors to
//              acquire balls.
////////////////////////////////////////////////////////////////////////////////
void DoorsAcquire()
{
  CloseOutDoor();
  OpenInDoor();
}

////////////////////////////////////////////////////////////////////////////////
// Function:    void DoorsRelease()
// Description: This function is used to change the position of the doors to 
//              release balls.
////////////////////////////////////////////////////////////////////////////////
void DoorsRelease()
{
  CloseInDoor();
  OpenOutDoor();
}

////////////////////////////////////////////////////////////////////////////////
// Function:    void CloseOutDoor()
// Description: This function is used to close the output door.
////////////////////////////////////////////////////////////////////////////////
void CloseOutDoor()
{
  mOutDoorServo.write( OUT_DOOR_CLOSED_ANGLE );
  delay( DOOR_DELAY_MS );
}

////////////////////////////////////////////////////////////////////////////////
// Function:    bool IsSwitchOn( unsigned int const pulseWidth )
// Description: This function is used to determine whether the fan switch is
//              in the ON Position.
////////////////////////////////////////////////////////////////////////////////
bool IsSwitchOn( unsigned int const pulseWidth )
{ 
  return( (pulseWidth > SWITCH_ON_MIN) && (pulseWidth < SWITCH_ON_MAX) ); 
}

////////////////////////////////////////////////////////////////////////////////
// Function:    bool IsSwitchOff( unsigned int const pulseWidth )
// Description: This function is used to determine whether the fan switch is 
//              in the OFF position.
////////////////////////////////////////////////////////////////////////////////
bool IsSwitchOff( unsigned int const pulseWidth )
{ 
  return( (pulseWidth > SWITCH_OFF_MIN) && (pulseWidth < SWITCH_OFF_MAX) ); 
}

////////////////////////////////////////////////////////////////////////////////
// Function:    bool ReadyToAcquire( unsigned int const pulseWidth )
// Description: This function is used to determine whether the door switch is
//              in the acquire position
////////////////////////////////////////////////////////////////////////////////
bool ReadyToAcquire( unsigned int const pulseWidth )
{ 
  return( (pulseWidth > SUCK_MIN) && (pulseWidth < SUCK_MAX) ); 
}

////////////////////////////////////////////////////////////////////////////////
// Function:    bool ReadyToRelease( unsigned int const pulseWidth )
// Description: This function is used to determine whether the door switch is
//              in the release position.
////////////////////////////////////////////////////////////////////////////////
bool ReadyToRelease( unsigned int const pulseWidth )
{ 
  return( (pulseWidth > BLOW_MIN) && (pulseWidth < BLOW_MAX) ); 
}

////////////////////////////////////////////////////////////////////////////////
// Function:    void FanBlow()
// Description: This function is used to command reverse thrust from the fan.
////////////////////////////////////////////////////////////////////////////////
void FanBlow()
{
  digitalWrite( BRIDGE_IN1_PIN, HIGH );
  digitalWrite( BRIDGE_IN2_PIN, LOW );
  digitalWrite( BRIDGE_EN_PIN , HIGH );
}

////////////////////////////////////////////////////////////////////////////////
// Function:    void FanSuck()
// Description: This function is used to command forward thrust from the fan.
////////////////////////////////////////////////////////////////////////////////
void FanSuck()
{
  digitalWrite( BRIDGE_IN1_PIN, LOW );
  digitalWrite( BRIDGE_IN2_PIN, HIGH );
  digitalWrite( BRIDGE_EN_PIN , HIGH );
}

////////////////////////////////////////////////////////////////////////////////
// Function:    void FanOff()
// Description: This function is used to turn the fan off.
////////////////////////////////////////////////////////////////////////////////
void FanOff()
{
  digitalWrite( BRIDGE_EN_PIN , LOW );
  digitalWrite( BRIDGE_IN1_PIN, LOW );
  digitalWrite( BRIDGE_IN2_PIN, LOW );
}

////////////////////////////////////////////////////////////////////////////////
// Function:    void RunDoorControl( unsigned int const doorsValue )
// Description: This function is used to control the motion of the doors
////////////////////////////////////////////////////////////////////////////////
void RunDoorControl( unsigned int const doorsValue )
{
  mPrevDoorsState = mCurrDoorsState;

  if( ReadyToRelease( doorsValue) )
  {
    mCurrDoorsState = RELEASE;
  }
  else if( ReadyToAcquire( doorsValue ) )
  {
    mCurrDoorsState = ACQUIRE;
  }

  if( mPrevDoorsState != mCurrDoorsState )
  {
    if( (mPrevDoorsState == ACQUIRE) && (mCurrDoorsState == RELEASE) )
    {
      // If the fan is currently on, we'll turn it off before we switch the 
      // position of the doors.
      if( mSystemState != IDLE )
      {
        FanOff();
      }
      
      DoorsRelease();
      
      if( (mSystemState != IDLE) && IsSwitchOn(mFanValue) )
      {
        FanBlow();
        mSystemState = RELEASING;
      }
    }
    else if( (mPrevDoorsState == RELEASE) && (mCurrDoorsState == ACQUIRE) )
    {
      // If the fan is currently on, we'll turn it off before we switch the 
      // position of the doors.
      if( mSystemState != IDLE )
      {
        FanOff();
      }
      
      DoorsAcquire();
      
      if( (mSystemState != IDLE) &&  IsSwitchOn(mFanValue) )
      {
        FanSuck();
        mSystemState = ACQUIRING;
      }      
    }
  }
}

////////////////////////////////////////////////////////////////////////////////
// Function:    void RunFanControl( unsigned int const fanSwitchValue )
// Description: This function is used to control the fan.
////////////////////////////////////////////////////////////////////////////////
void RunFanControl( unsigned int const fanSwitchValue )
{
  if( IsSwitchOn(fanSwitchValue) )
  { 
    switch( mSystemState )
    {
      case IDLE :
      {
        switch( mCurrDoorsState )
        {
          case ACQUIRE :
          {
            FanSuck();
            mSystemState = ACQUIRING;
            break ;
          }
          
          case RELEASE :
          {
            FanBlow();
            mSystemState = RELEASING;
            break ;
          }
          
          default :
          {
            FanBlow();
            mSystemState = RELEASING;
            break ;
          }
        }
        
        break;
      }
      
      default : break ;
    }
  }
  else if( IsSwitchOff(mFanValue)  )
  {   
    if( mSystemState != IDLE )
    {
      FanOff();
      mSystemState = IDLE; 
    }
  }
}

////////////////////////////////////////////////////////////////////////////////
// Function:    
// Description: 
////////////////////////////////////////////////////////////////////////////////
unsigned int ReadDoorsValue()
{
  unsigned int value = 0;
  
  #if SIM
  value = GetSerialValue();
  #else
  value = pulseIn (DOORS_TOGGLE_PIN, HIGH, 20000);
  #endif
  
  #if DEBUG
  Serial.print("Doors Value: ");
  Serial.println(value);
  #endif
  
  return( value );
}

////////////////////////////////////////////////////////////////////////////////
// Function:    
// Description: 
////////////////////////////////////////////////////////////////////////////////
unsigned int ReadFanValue()
{
  unsigned int value = 0;
  #if SIM
  value = GetSerialValue();
  #else
  value = pulseIn (FAN_TOGGLE_PIN  , HIGH, 20000);
  #endif
  
  #if DEBUG
  Serial.print("Fan Value: ");
  Serial.println(value);
  #endif
  
  return( value );
}

////////////////////////////////////////////////////////////////////////////////
// Function:    
// Description: 
////////////////////////////////////////////////////////////////////////////////
void FlushBuffer()
{
  for( unsigned int i = 0; i < BUFF_SIZE_BYTES; ++i )
  {
    mSerialBuffer[i] = 0;
  }
}

////////////////////////////////////////////////////////////////////////////////
// Function:    
// Description: 
////////////////////////////////////////////////////////////////////////////////
unsigned int GetSerialValue()
{
  unsigned int value = 0;
  while(1)
  {
    unsigned int const bytesAvailable = Serial.available() ;
    
    if( bytesAvailable >= 4 )
    {
      FlushBuffer();
      Serial.print("\nBytes available: ");
      Serial.println(bytesAvailable);
      unsigned int bytesRemaining = bytesAvailable;
      int i = 0;
      while( bytesRemaining > 0 )
      {
        mSerialBuffer[i] = Serial.read();
        --bytesRemaining;
        ++i;
      }
      value = atoi(mSerialBuffer);
      Serial.println(value);
      break ;
    }
  }
  
  return( value );
}

////////////////////////////////////////////////////////////////////////////////
// Function:    
// Description: 
////////////////////////////////////////////////////////////////////////////////
void setup()
{
  // Turn the Serial Port on
  Serial.begin(9600);
  
  mInDoorServo.attach(IN_DOOR_PIN);
  mOutDoorServo.attach(OUT_DOOR_PIN);

  // Setup the Input pins
  pinMode( DOORS_TOGGLE_PIN  , INPUT );
  pinMode( FAN_TOGGLE_PIN    , INPUT );

  // Setup the Output pins
  pinMode( BRIDGE_IN1_PIN, OUTPUT ); 
  pinMode( BRIDGE_IN2_PIN, OUTPUT ); 
  pinMode( BRIDGE_EN_PIN , OUTPUT ); 

  // Make sure that the fan if off
  FanOff();
  
  // Make sure the doors are in the acquire position
  DoorsAcquire();
}

////////////////////////////////////////////////////////////////////////////////
// Function:    
// Description: 
////////////////////////////////////////////////////////////////////////////////
void loop()
{
  mDoorsValue = ReadDoorsValue();
  mFanValue   = ReadFanValue();

  RunDoorControl( mDoorsValue );
  RunFanControl( mFanValue );
  

#if DEBUG
  switch( mSystemState )
  {
  case IDLE :
    {
      Serial.println("IDLE");
      break;
    }

  case RELEASING :
    {
      Serial.println("RELEASING");
      break;
    }

  case ACQUIRING :
    {
      Serial.println("ACQUIRING");
      break;
    }

  default : 
    break;
  }
  
  switch( mCurrDoorsState )
  {
  case ACQUIRE :
    {
      Serial.println("ACQUIRE");
      break;
    }

  case RELEASE :
    {
      Serial.println("RELEASE");
      break;
    }

  default : 
    break;
  }
#endif
}


